/*********************************************************************
 * INCLUDES
 */
#include <string.h>
#include "bcomdef.h"
#include "OSAL.h"
#include "OnBoard.h"
#include "gatt.h"
#include "gatt_uuid.h"
#include "gatt_profile_uuid.h"
#include "ancs.h"
#include "gattservapp.h"


uint8 ancsAppTaskId;

/*********************************************************************
 * MACROS
 */

// Length of Characteristic declaration + handle with 16 bit UUID 5+2bytes, for CC254x, 8bits address
#define CHAR_DESC_HDL_UUID128_LEN        21  // 5 + 16bytes = 21


// 7905F431-B5CE-4E99-A40F-4B1E122D00D0
#define ANCS_SVC_UUID 0xD0, 0x00, 0x2D, 0x12, 0x1E, 0x4B, 0x0F, 0xA4, 0x99, 0x4E, 0xCE, 0xB5, 0x31, 0xF4, 0x05, 0x79
// Notification Source: UUID 9FBF120D-6301-42D9-8C58-25E699A21DBD (notifiable)
#define ANCS_NOTIF_SRC_CHAR_UUID        0x1DBD // Last 2 bytes of the 128bit-16bytes UUID
// Control point: UUID 69D1D8F3-45E1-49A8-9821-9BBDFDAAD9D9 (writeable with response)
#define ANCS_CTRL_PT_CHAR_UUID          0xD9D9
// Data Source: UUID 22EAC6E9-24D6-4BB5-BE44-B36ACE7C7BFB (notifiable)
#define ANCS_DATA_SRC_CHAR_UUID         0x7BFB

/*********************************************************************
 * CONSTANTS
 */

/*********************************************************************
 * TYPEDEFS
 */

/*********************************************************************
 * GLOBAL VARIABLES
 */

/*********************************************************************
 * EXTERNAL VARIABLES
 */

// Attribute handle cache
uint16_t Ancs_handleCache[HDL_CACHE_LEN];
uint8_t serviceDiscoveryState = DISC_IDLE;

/*********************************************************************
 * EXTERNAL FUNCTIONS
 */

/*********************************************************************
 * LOCAL VARIABLES
 */

// Attribute handles used during discovery
static uint16_t Ancs_svcStartHdl;
static uint16_t Ancs_svcEndHdl;
static uint8_t Ancs_endHdlIdx;

/*********************************************************************
 * LOCAL FUNCTIONS
 */

static uint8_t Ancs_discover(uint8_t state, gattMsgEvent_t *pMsg);

/*********************************************************************
 * @fn      Ancs_discStart()
 *
 * @brief   Start service discovery. 
 *
 * @return  New discovery state.
 */
uint8_t Ancs_discStart(void)
{
  // Clear handle cache
  memset(Ancs_handleCache, 0, sizeof(Ancs_handleCache));
  
  // Start discovery with first service
  return Ancs_discGattMsg(DISC_ANCS_START, NULL);
}

/*********************************************************************
 * @fn      Ancs_discGattMsg()
 *
 * @brief   Handle GATT messages for characteristic discovery. 
 *
 * @param   state - Discovery state.
 * @param   pMsg  - GATT message.
 *
 * @return  New discovery state.
 */
uint8_t Ancs_discGattMsg(uint8_t state, gattMsgEvent_t *pMsg)
{
  // Execute discovery function for service
  do
  {
    switch (state & 0xF0)
    {
      // ANCS service
      case DISC_ANCS_START:
        state = Ancs_discover(state, pMsg);
        if (state == DISC_FAILED)
        {
          state = DISC_IDLE;
        }
        break;

      default:
        break;
    }       
  } while ((state != 0) && ((state & 0x0F) == 0));
  
  return state;
}

/*********************************************************************
 * @fn      Ancs_discover()
 *
 * @brief   Alert notification service and characteristic discovery. 
 *
 * @param   state - Discovery state.
 * @param   pMsg  - GATT message.
 *
 * @return  New discovery state.
 */
static uint8_t Ancs_discover(uint8_t state, gattMsgEvent_t *pMsg)
{
  uint8_t newState = state;
  static uint8_t isNotifCCCD = FALSE;
  
  switch (state)
  {
    case DISC_ANCS_START:  
      {
        uint8_t uuid[ATT_UUID_SIZE] = {ANCS_SVC_UUID};

        // Initialize service discovery variables
        Ancs_svcStartHdl = Ancs_svcEndHdl = 0;
        Ancs_endHdlIdx = 0;
        
        // Discover service by UUID
        GATT_DiscPrimaryServiceByUUID(Ancs_connHandle, uuid,
                                      ATT_UUID_SIZE, ancsAppTaskId);      

        newState = DISC_ANCS_SVC;
      }
      break;

    case DISC_ANCS_SVC:
      // Service found, store handles
      if (pMsg->method == ATT_FIND_BY_TYPE_VALUE_RSP &&
          pMsg->msg.findByTypeValueRsp.numInfo > 0)
      {
        Ancs_svcStartHdl = pMsg->msg.findByTypeValueRsp.handlesInfo[0].handle;
        Ancs_svcEndHdl   = pMsg->msg.findByTypeValueRsp.handlesInfo[0].grpEndHandle;
      }
      
      // If procedure complete
      if ((pMsg->method == ATT_FIND_BY_TYPE_VALUE_RSP  && 
           pMsg->hdr.status == bleProcedureComplete) ||
          (pMsg->method == ATT_ERROR_RSP))
      {
        // If service found
        if (Ancs_svcStartHdl != 0)
        {
          // Discover all characteristics
          GATT_DiscAllChars(Ancs_connHandle, Ancs_svcStartHdl,
                            Ancs_svcEndHdl, ancsAppTaskId);
          
          newState = DISC_ANCS_CHAR;
        }
        else
        {
          // Service not found
          newState = DISC_FAILED;
        }
      }    
      break;

    case DISC_ANCS_CHAR:
      {
        // Characteristics found
        uint8_t   i;
        uint8_t   *p;
        uint16_t  handle;
        uint16_t  uuid;
        if (pMsg->method == ATT_READ_BY_TYPE_RSP &&
            pMsg->msg.readByTypeRsp.numPairs > 0 && 
            pMsg->msg.readByTypeRsp.len == CHAR_DESC_HDL_UUID128_LEN)
        {
          
          p = pMsg->msg.readByTypeRsp.dataList;
          
          // For each characteristic declaration
          for (i = pMsg->msg.readByTypeRsp.numPairs; i > 0; i--)
          {
            // Parse characteristic declaration
            handle = BUILD_UINT16(p[3], p[4]);
            uuid = BUILD_UINT16(p[5], p[6]);
                   
            // If looking for end handle
            if (Ancs_endHdlIdx != 0)
            {
              // End handle is one less than handle of characteristic declaration
              Ancs_handleCache[Ancs_endHdlIdx] = BUILD_UINT16(p[0], p[1]) - 1;
              
              Ancs_endHdlIdx = 0;
            }

            // If UUID is of interest, store handle
            switch (uuid)
            {
              case ANCS_NOTIF_SRC_CHAR_UUID:
                Ancs_handleCache[HDL_ANCS_NTF_NOTIF_START] = handle;
                Ancs_endHdlIdx = HDL_ANCS_NTF_NOTIF_END;
                //LCD_WRITE_STRING("Found: Ntfi char", LCD_PAGE1);
                break;
                
              case ANCS_CTRL_PT_CHAR_UUID:
                Ancs_handleCache[HDL_ANCS_CTRL_PT_START] = handle;
                Ancs_endHdlIdx = HDL_ANCS_CTRL_PT_END;
                //LCD_WRITE_STRING("Found: Ctrl pt char", LCD_PAGE2);
                break;
                
              case ANCS_DATA_SRC_CHAR_UUID:
                Ancs_handleCache[HDL_ANCS_DATA_SRC_START] = handle;
                Ancs_endHdlIdx = HDL_ANCS_DATA_SRC_END;
                //LCD_WRITE_STRING("Found: data src char", LCD_PAGE3);
                break;
                
              default:
                break;
            }
            
            p += CHAR_DESC_HDL_UUID128_LEN;
          }
        }
          
        // If procedure complete
        if ((pMsg->method == ATT_READ_BY_TYPE_RSP  && 
             pMsg->hdr.status == bleProcedureComplete) ||
            (pMsg->method == ATT_ERROR_RSP))
        {
          // Special case of end handle at end of service
          if (Ancs_endHdlIdx != 0)
          {
            Ancs_handleCache[Ancs_endHdlIdx] = Ancs_svcEndHdl;
            Ancs_endHdlIdx = 0;
          }
          
          // If didn't find ANCS notification source characteristic, it is absolutely wrong
          if (Ancs_handleCache[HDL_ANCS_NTF_NOTIF_START] == 0)
          {
            newState = DISC_FAILED;
          }
          else if (Ancs_handleCache[HDL_ANCS_NTF_NOTIF_START] <
                   Ancs_handleCache[HDL_ANCS_NTF_NOTIF_END])
          {
            // Discover ANCS Notification Source characteristic client configuration
            GATT_DiscAllCharDescs(Ancs_connHandle,
                                  Ancs_handleCache[HDL_ANCS_NTF_NOTIF_START] + 1,
                                  Ancs_handleCache[HDL_ANCS_NTF_NOTIF_END],
                                  ancsAppTaskId);
            isNotifCCCD = TRUE;                            
            newState = DISC_ANCS_CCCD;
          }
          else
          {
            // Missing required characteristic descriptor
            Ancs_handleCache[HDL_ANCS_NTF_NOTIF_START] = 0;
            newState = DISC_FAILED;
          }
        }
        else // Discover all characteristics of ANCS service until end handle, to find what we need
        {
          GATT_DiscAllChars(Ancs_connHandle, handle+1, 
                            Ancs_svcEndHdl, ancsAppTaskId);

        }

      }      
      break;

    case DISC_ANCS_CCCD:
      {
        // Characteristic descriptors found
        if (pMsg->method == ATT_FIND_INFO_RSP &&
            pMsg->msg.findInfoRsp.numInfo > 0 && 
            pMsg->msg.findInfoRsp.format == ATT_HANDLE_BT_UUID_TYPE)
        {
          uint8_t i;
          
          // For each handle/uuid pair
          for ( i = 0; i < pMsg->msg.findInfoRsp.numInfo; i++ )
          {
            // Look for CCCD
            if ( (pMsg->msg.findInfoRsp.info.btPair[i].uuid[0] ==
                  LO_UINT16(GATT_CLIENT_CHAR_CFG_UUID)) &&
                 (pMsg->msg.findInfoRsp.info.btPair[i].uuid[1] ==
                  HI_UINT16(GATT_CHAR_EXT_PROPS_UUID)) )
            {
              // CCCD found
              Ancs_handleCache[HDL_ANCS_NTF_CCCD] =
                pMsg->msg.findInfoRsp.info.btPair[i].handle;
              newState = DISC_IDLE;
              break;
            }
          }
        }
        
        
        
        // If procedure complete
        if ((pMsg->method == ATT_FIND_INFO_RSP  && 
             pMsg->hdr.status == bleProcedureComplete) ||
            (pMsg->method == ATT_ERROR_RSP))
        {
          // Discover ANCS Data Source characteristic descriptors
          if (isNotifCCCD == TRUE && Ancs_handleCache[HDL_ANCS_DATA_SRC_CCCD] == 0)
          {
            GATT_DiscAllCharDescs(Ancs_connHandle,
                                  Ancs_handleCache[HDL_ANCS_DATA_SRC_START] + 1,
                                  Ancs_handleCache[HDL_ANCS_DATA_SRC_END],
                                  ICall_getEntityId());
 
            isNotifCCCD = FALSE;
            
          }
          else
          {
            newState = DISC_IDLE;
          }
        }
      }
      break;

    default:
      break;
  }
  // For Get Notificaion Attribute statement use
  serviceDiscoveryState = newState;
  
  return newState;
}


